#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

#include <boost/bind.hpp>
#include <boost/thread.hpp>

#include "cubic_spline.h"


#define PREVIEW_DIS 3  //预瞄距离
#define L 1.868  //轴距 l_r: 0.945  l_f: 0.923

using namespace std;

ros::Publisher ctl_cmd_pub;     // 向模型发送控制命令
ros::Publisher real_path_pub_;
nav_msgs::Path real_path;       // 小车实际运动Path
boost::mutex traj_mutex_;

float carVelocity = 0;
float l_d = 0;            // 前视距离
float k = 0.1;            // 计算前视距离的系数
vector<geometry_msgs::Point> traj;    // 要跟踪的轨迹
int pointNum = 0;         // 保存路径点的个数
int min_way_point = 20;   // 要跟踪的轨迹最小长度

// 计算四元数转换到欧拉角
std::array<float, 3> calQuaternionToEuler(const float x, const float y,
                                          const float z, const float w) {
  std::array<float, 3> calRPY = {(0, 0, 0)};
  // roll = atan2(2(wx+yz),1-2(x*x+y*y))
  calRPY[0] = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
  // pitch = arcsin(2(wy-zx))
  calRPY[1] = asin(2 * (w * y - z * x));
  // yaw = atan2(2(wx+yz),1-2(y*y+z*z))
  calRPY[2] = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));

  return calRPY;
}

/**
 * 计算发送给模型车的转角（可以看做控制模块的输出）
*/
void poseCallback(const geometry_msgs::PoseStamped &msg) {
  if (traj.size() < min_way_point) return;
  auto cur_posi_x = msg.pose.position.x;
  auto cur_posi_y = msg.pose.position.y;
  auto cur_posi_z = 0.0;

  auto currentQuaternionX = msg.pose.orientation.x;
  auto currentQuaternionY = msg.pose.orientation.y;
  auto currentQuaternionZ = msg.pose.orientation.z;
  auto currentQuaternionW = msg.pose.orientation.w;

  // 计算小车当前欧拉角
  std::array<float, 3> calRPY =
      calQuaternionToEuler(currentQuaternionX, currentQuaternionY,
                           currentQuaternionZ, currentQuaternionW);
  traj_mutex_.lock();
  // 通过计算当前坐标和目标轨迹距离，找到一个最小距离的索引号
  int index;
  vector<float> bestPoints_;
  // 遍历所有路径点和当前位置的距离，保存到数组中
  for (int i = 0; i < pointNum; i++) {
    float lad = sqrt(pow(traj[i].x - cur_posi_x, 2) + pow(traj[i].y - cur_posi_y, 2));
    bestPoints_.push_back(lad);
  }
  // 找到数组中最小横向距离
  auto smallest = min_element(bestPoints_.begin(), bestPoints_.end());
  // 找到最小横向距离的索引位置
  index = distance(bestPoints_.begin(), smallest);

  // 计算小车与最近参考点距离
  float delta_l = sqrt(pow(traj[index].x - cur_posi_x, 2) + pow(traj[index].y - cur_posi_y, 2));

  while (l_d > delta_l && (index + 1) < pointNum) {
    delta_l = sqrt(pow(traj[index].x - cur_posi_x, 2) + pow(traj[index].y - cur_posi_y, 2));
    index += 1;
  }

  float alpha = atan2(traj[index].y - cur_posi_y, traj[index].x - cur_posi_x) - calRPY[2];
  float theta = atan(2 * L * sin(alpha) / delta_l);  

  traj_mutex_.unlock();
  // 发布小车运动指令及运动轨迹
  if (delta_l > 0.2) {
    if (real_path.poses.size() > 2500)
      real_path.poses.clear();

    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x = 3;           // 恒定速度运动
    vel_msg.angular.z = theta;      // yaw 偏航角
    ctl_cmd_pub.publish(vel_msg);
    // 发布小车运动轨迹
    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = cur_posi_x;
    this_pose_stamped.pose.position.y = cur_posi_y;

    geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(theta);
    this_pose_stamped.pose.orientation.x = currentQuaternionX;
    this_pose_stamped.pose.orientation.y = currentQuaternionY;
    this_pose_stamped.pose.orientation.z = currentQuaternionZ;
    this_pose_stamped.pose.orientation.w = currentQuaternionW;

    this_pose_stamped.header.stamp = ros::Time::now();

    this_pose_stamped.header.frame_id = "world";
    real_path.poses.push_back(this_pose_stamped);
  } else {
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x = 0;
    vel_msg.angular.z = 0;
    ctl_cmd_pub.publish(vel_msg);
  }
  real_path_pub_.publish(real_path);
}

/**
 * 订阅车辆速度
*/
void velocityCall(const geometry_msgs::TwistStamped &msg) {
  carVelocity = msg.twist.linear.x;   // x 为车辆本身纵向速度
  l_d = k * carVelocity + PREVIEW_DIS;
}

/**
 * 订阅参考线话题，将要跟踪的数据保存
*/
void pointCallback(const nav_msgs::Path &msg) {
  pointNum = msg.poses.size();
  if (pointNum < min_way_point)  return;

  traj_mutex_.lock();
  // 因为每次规划的轨迹是不一样的，所以需要清空
  traj.clear();
  for (int i = 0; i < pointNum; i++) {
    geometry_msgs::Point tmp_point;
    tmp_point.x = msg.poses[i].pose.position.x;
    tmp_point.y = msg.poses[i].pose.position.y;
    tmp_point.z = 0.0;
    traj.push_back(tmp_point);
  }
  traj_mutex_.unlock();
}


int main(int argc, char **argv) {
  std::string tracking_topic;
  //创建节点
  ros::init(argc, argv, "pure_pursuit");
  ros::param::get("tracking_topic_name", tracking_topic);
  //创建节点句柄
  ros::NodeHandle n;
  //创建Publisher，发送经过pure_pursuit计算后的转角及速度
  ctl_cmd_pub = n.advertise<geometry_msgs::Twist>("/smart/cmd_vel", 20);
  real_path_pub_ = n.advertise<nav_msgs::Path>("rvizpath", 100, true);
  // ros::Rate loop_rate(10);

  real_path.header.frame_id = "world";
  real_path.header.stamp = ros::Time::now();   // 设置时间戳

  geometry_msgs::PoseStamped pose;
  pose.header.stamp = ros::Time::now();
  pose.header.frame_id = "world";

  ros::Subscriber splinePath = n.subscribe(tracking_topic, 20, pointCallback);
  ros::Subscriber carVel = n.subscribe("/smart/velocity", 20, velocityCall);
  ros::Subscriber carPose = n.subscribe("/smart/rear_pose", 20, poseCallback);
  ros::spin();
  return 0;
}
